/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.macro;

import com.googlecode.gunncsnavigation.telemetry.TelemetryPorts;
import com.grt192.actuator.spot.GRTDemoLED;
import com.grt192.controller.CreateTeleop;
import com.grt192.core.GRTObject;
import com.grt192.core.controller.Macro;
import com.grt192.mechanism.CreateBase;
import com.grt192.mechanism.SPOTDriverStation;
import com.grt192.mechanism.spot.SLight;
import com.grt192.prototyper.PrototyperUtils;
import com.grt192.sensor.spot.NetSensor;
import com.grt192.utils.Util;
import com.iaroc.irobot.IRobotCreate;

/**
 * TeleopDriver drives arcade style by reading a DemoJoystick from a Telemetry
 * Server.
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class TeleopDriver extends GRTObject implements Macro, TelemetryPorts {

    private SLight[] lights = SLight.get();
    private int createLight = 2;
    private int joystickLight = 7;
    //base
    IRobotCreate irc;
    CreateBase base;
    //ds
    NetSensor joystick;
    SPOTDriverStation ds;
    //controller
    CreateTeleop op;

    public TeleopDriver() {
        setPrinting(true);
    }

    public int execute() {
        log("Initializing Base...");
        lights[createLight].rawColor(GRTDemoLED.Color.ORANGE);
        irc = new IRobotCreate();
        base = new CreateBase(irc);
        base.setPrinting(true);
        lights[createLight].rawColor(GRTDemoLED.Color.GREEN);
        log("Base init success!");

        log("Initializing Driver...");
        joystick = new NetSensor(PrototyperUtils.getAddress(1, lights[joystickLight]), JOYSTICK_PORT);
        joystick.start();
        ds = new SPOTDriverStation(joystick);
        log("Driver init success!");

        log("Initializing controller...");
        op = new CreateTeleop(base, ds);
        log("Starting controller...");
        op.start();



        while (true) {
            Util.sleep(100000);
        }

    }
}
